#include "rclcpp/rclcpp.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"


using ChangeState = lifecycle_msgs::srv::ChangeState;
using ChangeStateClient = rclcpp::Client<ChangeState>;
using Transition = lifecycle_msgs::msg::Transition;

class MoveRobotNodeStartup : public rclcpp::Node
{
public:
  MoveRobotNodeStartup() : Node("lifecycle_manager")
  {
    this->declare_parameter<std::vector<std::string>>("managed_node_names");
    node_name_list =  this->get_parameter("managed_node_names").as_string_array();

    RCLCPP_INFO(this->get_logger(), "Manage robot node has been started");

    initialization_sequence();
  }

private:
  void change_state(const Transition transition)
  {
    size_t node_numbers = node_name_list.size();
    for (size_t i=0; i < node_numbers; i++)
    {
      std::string service_name = node_name_list[i] + "/change_state";  
      RCLCPP_INFO(this->get_logger(), "Node name %s", service_name.c_str());
      auto client = this->create_client<ChangeState>(service_name);  
      // std::chrono::seconds(1)
      if (!client->wait_for_service())
      {
        RCLCPP_ERROR(this->get_logger(), "Service not available after wait");
        continue;
      }
    
      // RCLCPP_ERROR(this->get_logger(), "Service available");
      auto request = std::make_shared<ChangeState::Request>();
      request->transition = transition;

      auto future = client->async_send_request(request);
      
      // 等待异步请求完成
      if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == 
        rclcpp::FutureReturnCode::SUCCESS)
      {
        try
        {
          auto response = future.get();
          // 处理响应（如果需要）
          RCLCPP_INFO(this->get_logger(), "Received response from service");
        }
        catch (const std::exception &e)
        {
          RCLCPP_ERROR(this->get_logger(), "Service call failed: %s", e.what());
        }
      }
      else
      {
        RCLCPP_ERROR(this->get_logger(), "Service call timed out");
      }
    }
  }

  void initialization_sequence() {
    // Unconfigured to Inactive (假设Inactive状态在这里是隐式的，因为直接配置了)
    RCLCPP_INFO(this->get_logger(),"Try to switch to configuring");
    Transition transition = Transition();
    transition.id = transition.TRANSITION_CONFIGURE;
    transition.label = "configure";
    change_state(transition);
    RCLCPP_INFO(this->get_logger(),"Configuring OK, now inactive (simulated)");

    // 模拟一些处理时间
    std::this_thread::sleep_for(std::chrono::seconds(3));

    // Inactive to Active
    RCLCPP_INFO(this->get_logger(),"Try to switch to activating");
    transition.id = transition.TRANSITION_ACTIVATE;
    transition.label = "activate";
    change_state(transition);
    RCLCPP_INFO(this->get_logger(), "Activating OK, now active");
  }

  std::vector<std::string> node_name_list;
  std::mutex mutex;
};


int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<MoveRobotNodeStartup>();
  rclcpp::spin(node);
  rclcpp::shutdown();

  return 0;
}